/*
 * @brief:
 *      度目电机对象，对于1个机头，每组电机包含两个电机
 *      q1,q2为0组,h1,h2为1组，q2,q3为2组,h2,h3为3组，qxk,hxk为4组,qxk2,hxk2为5组
 *
 * ChangeLog        Author          Notes
 * 2015.09.09       Herbert         First Version
 * 2016.03.01       Herbert         增加qxk2,hxk2
 */
#include "mhdef.h"
#include "stepmotor.h"
#include "motor.h"
#include "ccycle.h"
#include <assert.h>
#include <malloc.h>
#include <string.h>
#include <stdio.h>

#define MOTOR_CYCLE_TIME    500

MOTOR* motor_new()
{
    int i = 0;
    MOTOR *motor = (MOTOR*)malloc(sizeof (MOTOR));

    ccycle_init(&motor->cycle);
    ccycle_init(&motor->reset_cycle);

    timer_init(&motor->timer);

    for (i = 0; i < MNUM; i++)
    {
        motor->pos[0][i] = 0;
        motor->pos[1][i] = 0;
        motor->pos_bk[0][i] = 0;
        motor->pos_bk[1][i] = 0;
    }

    motor->setting_pos[0] = 300;
    motor->setting_pos[1] = 300;

    motor->reset = stepmotor_reset;
    motor->exec = stepmotor_exec;

    motor->restore = FALSE;
    motor->reset_all = FALSE;
    motor->sink_enable = TRUE;

    return motor;
}

void motor_set_cycle(MOTOR *motor, CCYCLE *cycle, CCYCLE *reset_cycle)
{
    assert(motor != NULL);
    assert(cycle != NULL);
    assert(reset_cycle != NULL);

    memcpy(&motor->cycle, cycle, sizeof (CCYCLE));
    memcpy(&motor->reset_cycle, reset_cycle, sizeof (CCYCLE));
}

/*
 * @param motor: 电机对象
 * @param type: 动作类型，动作或者复位
 * @note: 执行cycle对应的group和index，循环只执行左机头或者只执行右机头
 */
int motor_exec(MH_UDP *pudp, MOTOR *motor, ActType type)
{
    assert(motor != NULL);

    short delta[MNUM] = { 0 };
    short find_zero[MNUM] = { 0 };
    int i = 0;
    int j = 0;
    int index = 0;
    int ctype = LEFT_CARRIAGE;

    CCYCLE *cycle = NULL;

    if (ccycle_get_state(&motor->reset_cycle) != CYCLE_STOP)
    {
        type = RESET;
        cycle = &motor->reset_cycle;
    }

    if (ccycle_get_state(&motor->cycle) != CYCLE_STOP)
    {
        type = ACT;
        cycle = &motor->cycle;
    }

    if (cycle == NULL)
    {
        return -1;
    }

    short start_group = ccycle_get_start_group(cycle);
    short end_group = ccycle_get_end_group(cycle);
    short start_index = ccycle_get_start_index(cycle);
    short end_index = ccycle_get_end_index(cycle);

    if (start_group >= GNUM)
    {
        ctype = RIGHT_CARRIAGE;
    }

    start_group -= ctype * GNUM;
    end_group -= ctype * GNUM;

    if (type == RESET)
    {
        for (i = start_group; i <= end_group; i++)
        {
            for (j = start_index; j <= end_index; j++)
            {
                index = i * 2 + j;

                motor->pos[ctype][index] = 0;
                if ((index >= 8 && index < MNUM) && !motor->sink_enable)
                {
                    continue;
                }
                find_zero[index] = TRUE;
            }
        }
    }
    else
    {
        short des_pos = 0;
        for (i = start_group; i <= end_group; i++)
        {
            for (j = start_index; j <= end_index; j++)
            {
                index = i * 2 + j;

                printf("sink enable = %d\n", motor->sink_enable);

                if ((index >= 8 && index < MNUM) && !motor->sink_enable)
                {
                    continue;
                }

                if (motor->restore)
                {
                    des_pos = motor->pos_bk[ctype][index];
                }
                else
                {
                    if (motor->pos[ctype][index] == 0)
                    {
                        des_pos = motor->setting_pos[index >= 8];
                    }
                    else
                    {
                        des_pos = 0;
                        find_zero[index] = TRUE;
                    }
                }

                printf("index = %d, des_pos = %d\n", index, des_pos);


                delta[index] = des_pos - motor->pos[ctype][index];
                motor->pos[ctype][index] = des_pos;
            }
        }

    }

#if 1
    printf("%d %d %d %d %d %d %d %d\n",
        delta[0], delta[1], delta[2], delta[3], \
        delta[4], delta[5], delta[6], delta[7]);

#endif

    if (type == RESET)
    {
        return motor->reset(pudp, ctype, find_zero, MNUM);
    }

    return motor->exec(pudp, ctype, delta, find_zero, MNUM);
}

int motor_exec_group(MH_UDP *pudp, MOTOR *motor, int group)
{
    assert(motor != NULL);

    short delta[MNUM] = { 0 };
    short find_zero[MNUM] = { 0 };
    int ctype = LEFT_CARRIAGE;
    int pos_setting = group == 4 ? motor->setting_pos[0] : motor->setting_pos[1];  //判断度目还是辛克

    uchar group_state = motor_get_group_state(motor, group);
     if (group_state == DOWN)
    {
        delta[group * 2] = pos_setting;
        delta[group *2 + 1] = pos_setting;
        find_zero[group *2] = DOWN;
        find_zero[group *2 + 1] = DOWN;
    }
    else
    {
        delta[group *2] = -pos_setting;
        delta[group *2 + 1] = -pos_setting;
        find_zero[group *2] = UP;
        find_zero[group *2 + 1] = UP;
    }

    return motor->exec(pudp, ctype, delta, find_zero, MNUM);
}
/*
 * @param motor: 电机对象
 * @param cycle: 当前执行的循环
 * @param type: 动作类型
 */
void motor_update_cycle(MOTOR *motor, CCYCLE *cycle, ActType type)
{
    assert(motor != NULL);
    assert(cycle != NULL);

    if (motor->reset_all || motor->restore)
    {
        short group = ccycle_get_group(cycle);
        short end_group = ccycle_get_end_group(cycle);

        group += GNUM;

        if (group >= end_group)
        {
            group = 0;
            ccycle_set_group(cycle, group);
            ccycle_stop(cycle);
        }
        else
        {
            ccycle_set_group(cycle, group);
            ccycle_set_start_group(cycle, GNUM);
            ccycle_set_end_group(cycle, GNUM + GNUM - 1);
        }
    }
    else if (type == RESET)
    {
        /* 复位动作只执行一次 */
        ccycle_stop(cycle);
    }
}

/*
 * @param user_data: 电机对象
 * @note: 执行循环
 */
int motor_exec_cycle(MH_UDP *pudp, void *user_data)
{
    assert(user_data != NULL);

    MOTOR *motor = (MOTOR*)user_data;
    int group = -1;
    int index = -1;
    ActType type = ACT;

    CCYCLE *cycle = NULL;

    if (ccycle_get_state(&motor->reset_cycle) != CYCLE_STOP)
    {
        type = RESET;
        cycle = &motor->reset_cycle;
    }

     if (ccycle_get_state(&motor->cycle) != CYCLE_STOP)
    {
        type = ACT;
        cycle = &motor->cycle;
    }

    if (cycle == NULL)
    {
        return 0;
    }

    group = cycle->group;
    index = cycle->index;

    if (ccycle_get_state(cycle) == CYCLE_READY)
    {
        ccycle_set_state(cycle, CYCLE_EXEC);
    }
    else
    {
        FEEDBACK fb;
        group = ccycle_get_start_group(cycle);

        int ret = feedback_get_error_state(pudp, &fb, (group >= GNUM));

        if (ret < 0)
        {
            return ret;
        }

        ret = motor_encount_error(motor, &fb, cycle);

        if (ret != 0)
        {
            printf("encount error\n");
            return ret;
        }

        motor_update_cycle(motor, cycle, type);

        if (type == RESET)
        {
            if (ccycle_get_state(&motor->cycle) != CYCLE_STOP)
            {
                /* 执行动作循环 */
                timer_start(&motor->timer, MOTOR_CYCLE_TIME, MOTOR_CYCLE_TIME, motor_exec_cycle);
                return 0;
            }
            return 0;
        }
        printf("motor restore state:%d\n",motor->restore);
         if (motor->restore)
        {
            /* 恢复状态执行到这里结束，只执行一次 */
            motor_stop_cycle(motor);
            return 0;
        }
    }

    if (group < 0 || index < 0)
    {
        return -1;
    }

    timer_start(&motor->timer, 0, MOTOR_CYCLE_TIME, motor_exec_cycle);
    return motor_exec(pudp, motor, type);
}

/*
 * @note: 用于检测单个测试的错误
 */
int motor_check_error(MH_UDP *pudp, void *user_data)
{
    assert(user_data != NULL);
    MOTOR *motor = (MOTOR*)user_data;
    FEEDBACK fb;
    int ret = 0;
    int group = ccycle_get_group(&motor->cycle);

    memset(&fb, 0, sizeof (FEEDBACK));

    ret = feedback_get_error_state(pudp , &fb, (group >= GNUM));
    if (ret < 0)
    {
        return ret;
    }

    return motor_encount_error(motor, &fb, &motor->cycle);
}

int motor_exec_timer(MH_UDP *pudp, MOTOR *motor)
{
    assert(motor != NULL);
    return timer_exec(pudp, &motor->timer, (void*)motor);
}

/*
 * @param motor: 度目电机对象，unused
 * @param pos_in: 下位机的电机位置，按照q1,q2,h1,h2,qxk,hxk,q3,q4,h3,h4,qxk2,hxk2排列
 * @param pos_out: 转换之后的度目位置，按照q1,q2,h1,h2,q3,q4,h3,h4,qxk,hxk,qxk2,hxk2排列
 * @note: 将从下位机读出来的电机位置转换为上位机的电机位置
 */
void motor_convert_pos(MOTOR *motor, short *pos_in, short *pos_out)
{
    assert(pos_in != NULL);
    assert(pos_out != NULL);

    int i = 0;
    short pos_in_tmp[MNUM] = { 0 };

    memcpy(pos_in_tmp, pos_in, MNUM * sizeof (short));

    /* 下位机顺序
        q1,q2,h1,h2,qxk,hxk,q3,q4,h3,h4　*/
    /* 上位机顺序
        q1,q2,h1,h2,q3,q4,h3,h4,qxk,hxk */

    for (i = 0; i < 4; i++)
    {
        pos_out[i] = pos_in_tmp[i];
    }

    /* 下位机转换到上位机 */
    for (i = 0; i < 4; i++)
    {
        pos_out[4 + i] = pos_in_tmp[6 + i];
    }

    pos_out[8] = pos_in_tmp[4];
    pos_out[9] = pos_in_tmp[5];
    pos_out[10] = pos_in_tmp[10];
    pos_out[11] = pos_in_tmp[11];
}

/*
 * @param motor: 纱嘴对象
 * @param pos: 从下位机读取到的电机位置
 * @param num: 电机个数
 */
void motor_set_pos(MOTOR *motor, short *pos, int num)
{
    assert(motor != NULL);
    assert(pos != NULL);

    motor_convert_pos(motor, pos, motor->pos[0]);
    if (num == MNUM * 2)
    {
        motor_convert_pos(motor, pos + MNUM, motor->pos[1]);
    }

    memcpy(motor->pos_bk, motor->pos, sizeof (motor->pos));

}

/*
 * @param motor: 电机对象
 * @param pos[out]:获取到的电机位置
 * @param num: 电机个数
 */
void motor_get_pos(MOTOR *motor, short *pos, int num)
{
    assert(motor != NULL);
    assert(pos != NULL);

    if (num > MNUM * 2)
    {
        return;
    }

    memcpy(pos, motor->pos, num * sizeof (short));
}

void motor_set_group_state(MOTOR *motor, short state, int group)
{
    assert(motor != NULL);
    motor->group_state[group] = state;
}

short motor_get_group_state(MOTOR *motor, int group)
{
    assert(motor != NULL);

    return motor->group_state[group];
}

void motor_stop_cycle(MOTOR *motor)
{
    assert(motor != NULL);
    timer_stop(&motor->timer);
    ccycle_stop(&motor->reset_cycle);
    ccycle_stop(&motor->cycle);
}

/*
 * @param motor: 电机对象
 * @param fb: 反馈
 * @param cycle: 当前执行的循环
 * @note: 判断是否遇到错误
 */
int motor_encount_error(MOTOR *motor, FEEDBACK *fb, CCYCLE *cycle)
{
    assert(motor != NULL);
    assert(fb != NULL);
    assert(cycle != NULL);

    int i = 0;
    int j = 0;
    int start_group = ccycle_get_start_group(cycle);
    int end_group = ccycle_get_end_group(cycle);
    int start_index = ccycle_get_start_index(cycle);
    int end_index = ccycle_get_end_index(cycle);
    int ctype = LEFT_CARRIAGE;
    if (start_group >= GNUM)
    {
        start_group -= GNUM;
        end_group -= GNUM;
        ctype = RIGHT_CARRIAGE;
    }
    /* 调整反馈顺序，按照电机顺序排列 */
    char state[MNUM] = { 0 };
    state[0] = fb->error.q1;
    state[1] = fb->error.q2;
    state[2] = fb->error.h1;
    state[3] = fb->error.h2;
    state[4] = fb->error.q3;
    state[5] = fb->error.q4;
    state[6] = fb->error.h3;
    state[7] = fb->error.h4;
    state[8] = fb->error.qxk;
    state[9] = fb->error.hxk;
    state[10] = fb->error.qxk2;
    state[11] = fb->error.hxk2;

    for (i = start_group; i <= end_group; i++)
    {
        for (j = start_index; j <= end_index; j++)
        {
            int index = i * 2 + j;
            if (index >= MNUM)
            {
                continue;
            }

            if (state[index])
            {
                if (ctype == RIGHT_CARRIAGE)
                {
                    return CRGERROR_F1_R - index;
                }

                return CRGERROR_F1 - index;
            }
        }
    }
    return 0;
}

BOOL motor_reset_complete(MOTOR *motor)
{
    assert(motor != NULL);

    return (ccycle_get_state(&motor->reset_cycle) == CYCLE_STOP);
}

BOOL motor_restore_complete(MOTOR *motor)
{
    assert(motor != NULL);

    BOOL complete = FALSE;
    if ((ccycle_get_state(&motor->reset_cycle) == CYCLE_STOP) &&
        (ccycle_get_state(&motor->cycle) == CYCLE_STOP))
    {
        complete = TRUE;
    }

    return complete;
}

void motor_start_timer(MOTOR *motor, TIMEOUT_FUNC pfunc, BOOL now)
{
    assert(motor != NULL);
    if (now)
    {
        timer_start(&motor->timer, MOTOR_CYCLE_TIME, MOTOR_CYCLE_TIME, pfunc);
    }
    else
    {
        timer_start(&motor->timer, 0, MOTOR_CYCLE_TIME, pfunc);
    }
}

void motor_setting_pos(MOTOR *motor, short dumu_pos, short sink_pos)
{
    assert(motor != NULL);
    motor->setting_pos[0] = dumu_pos;
    motor->setting_pos[1] = sink_pos;
}

void motor_set_restore(MOTOR *motor, BOOL restore)
{
    assert(motor != NULL);
    motor->restore = restore;
}

void motor_set_reset_all(MOTOR *motor, BOOL reset_all)
{
    assert(motor != NULL);
    motor->reset_all = reset_all;
}

void motor_set_sink_enable(MOTOR *motor, BOOL enable)
{
    assert(motor != NULL);
    motor->sink_enable = enable;
}

CCYCLE* motor_get_cycle(MOTOR *motor)
{
    assert(motor != NULL);
    return &motor->cycle;
}

BOOL motor_need_restore(MOTOR *motor)
{
    assert(motor != NULL);
    return memcmp(motor->pos, motor->pos_bk, sizeof (motor->pos));
}
